from machine import Pin, PWM
led11 = Pin(16, Pin.OUT)
led22 = Pin(17, Pin.OUT)
led11.value(1)

led = Pin(33, Pin.IN)
led1 = Pin(25, Pin.IN)
led2 = Pin(14, Pin.IN)
led4 = Pin(12, Pin.IN)
motor1 = PWM(Pin(32))
motor2 = PWM(Pin(26))
motor3 = PWM(Pin(27))
motor4 = PWM(Pin(13))
motor1.init(freq=1000, duty=0)
motor2.init(freq=1000, duty=0)
motor3.init(freq=1000, duty=0)
motor4.init(freq=1000, duty=0)

while True:
    led22.value(0)
'''
class PIDController:
    def __init__(self, P, I, D, setpoint):
        self.Kp = P#比例项参数 Kp： 控制系统的灵敏度，如果设置太高，系统可能会过于敏感，引起振荡。如果设置太低，系统响应可能较慢。
        self.Ki = I#积分项参数 Ki： 控制系统的稳定性，用于消除系统静差。过高的积分项可能导致系统过度调整，产生超调。
        self.Kd = D#微分项参数 Kd： 控制系统的过冲和振荡，可以减缓系统的响应。然而，过高的微分项可能引入噪声。
        self.setpoint = setpoint
        self.last_error = 0
        self.integrated_error = 0
        self.output = 0

    def calculate(self, actual_value):
        error = self.setpoint - actual_value
        self.integrated_error += error
        derivative = error - self.last_error
        self.last_error = error
        # PID equation
        self.output = (self.Kp * error) + (self.Ki * self.integrated_error) + (self.Kd * derivative)
        return self.output

# 使用PID控制器的例子
pid = PIDController(P=1, I=0.01, D=0, setpoint=100)
for x in range(100):
    output = pid.calculate(x)
    print(output)
'''